/**
  ******************************************************************************
  * @file    Project/STM32F4xx_StdPeriph_Templates/main.c 
  * @author  MCD Application Team
  * @version V1.5.0
  * @date    06-March-2015
  * @brief   Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; COPYRIGHT 2015 STMicroelectronics</center></h2>
  *
  * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
  * You may not use this file except in compliance with the License.
  * You may obtain a copy of the License at:
  *
  *        http://www.st.com/software_license_agreement_liberty_v2
  *
  * Unless required by applicable law or agreed to in writing, software 
  * distributed under the License is distributed on an "AS IS" BASIS, 
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stdio.h"
#include "bsp.h"
#include "bsp_mb_host.h"
#include "RS485.h"
#include "icm40607.h"
#include "app_sensor.h"
#include "drv_can.h"
#include "drv_st485.h"
#include "ioi2c.h"
#include "ist8310.h"
#include "task_config.h"
#include "rtthread.h"

/** @addtogroup Template_Project
  * @{
  */ 

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
static __IO uint32_t uwTimingDelay;

RCC_ClocksTypeDef RCC_Clocks;
//u8 Printdata[10] = {0x30,0x31,0x32,0x33,0x34,0x35,0x36,0x37,0x38,0x39};
Rx_MSG_STATE Rx_MSG  = MSG_IDLE;   // 接收报文状态
u32 ResponTimeOut_cnt = 0;
u8 ModbusSlave_ADDR,ModbusRegReadADDR,ModbusReadREGNums = 0x0;//初始化从机地址和从机初始读取寄存器地址
u16 MBSlaveREG_var[8][80]={0};//从机寄存器缓存,每个寄存器2个字节16bit
	  
static rt_thread_t tid1 = RT_NULL;
static rt_thread_t tid2 = RT_NULL;
static rt_thread_t tid3 = RT_NULL;
static rt_thread_t tid4 = RT_NULL;
static rt_thread_t tid5 = RT_NULL;
static rt_thread_t tid6 = RT_NULL;
static rt_thread_t tid7 = RT_NULL;
static rt_thread_t tid8 = RT_NULL;
static rt_thread_t tid9 = RT_NULL;

rt_sem_t rt_sem_sensor = RT_NULL;//定义一个消息指针
rt_sem_t rt_sem_motion = RT_NULL;//定义一个消息指针
rt_sem_t rt_sem_magnect = RT_NULL;//定义一个消息指针
rt_sem_t rt_sem_monitor = RT_NULL;//定义一个消息指针
rt_sem_t rt_sem_CANsendempty = RT_NULL;//定义CAN发送缓存空消息指针
rt_sem_t rt_sem_IMUPrintPause = RT_NULL;//定义一个消息指针
rt_sem_t rt_sem_STUartSendempty = RT_NULL;//定义UART发送缓存空消息指针

//rt_mailbox_t CAN_Send_mailbox = RT_NULL;
//rt_mailbox_t STUartRx_mailbox = RT_NULL;

rt_mq_t  rt_mq_CAN_Send = RT_NULL;
rt_mq_t  rt_mq_CAN_Rec	= RT_NULL;
rt_mq_t  rt_mq_STmotor_send = RT_NULL;
rt_mq_t  rt_mq_STmotor_rec = RT_NULL;

/* Private function prototypes -----------------------------------------------*/
void Delay(u16 nTime);
void Function_Task_create (void);
void Function_semaphore_init(void);
void Function_mailbox_init(void);
void Function_msg_queue_init(void);
float getsystimestamp(void);
//static char Sensor_task_stack[1024];
//static struct rt_thread Sensor_task;
u8 imu_who_am_i;
u8 imu_res[14];
//u8 SendData[8] = {1,2,3,4,5,6,7,8};

void SensorTask_entry(void* parameter);
void MotionTask_entry(void* parameter);
void MagnectTask_entry(void* parameter);
void MonitorTask_entry(void* parameter);
void CAN_SendTask_entry(void* parameter);
void CAN_RecTask_entry(void* parameter);
void STUart_SendTask_entry(void* parameter);
//void STUart_RecTask_entry(void* parameter);

float systime;
extern CanTxMsg TxMsg;
extern SYSTEM_STATUS g_tSysStatus;
//u32 sendteset = 0x12345678;
//extern RTC_TimeTypeDef GetTimeStrct;//获取时间结构体
//extern u32 RTC_SubSecond;//亚秒
//extern RTC_DateTypeDef GetDateStruct;
/**
  * @brief  Main program
  * @param  None
  * @retval None
  */
int main(void)
{
//	u32 BaudRate = 9600;
//  u8 ReadSensor_index;
//	u8 ReadSensor_cnt = 2;
	
  /*!< At this stage the microcontroller clock setting is already configured, 
       this is done through SystemInit() function which is called from startup
       files (startup_stm32f40_41xxx.s/startup_stm32f427_437xx.s/
       startup_stm32f429_439xx.s/startup_stm32f401xx.s/startup_stm32f411xe.s or
       startup_stm32f446xx.s)
       before to branch to application main.
       To reconfigure the default setting of SystemInit() function, 
       refer to system_stm32f4xx.c file */

  /* SysTick end of count event each 10ms */
  RCC_GetClocksFreq(&RCC_Clocks);
//  SysTick_Config(RCC_Clocks.HCLK_Frequency / 1000);//systick 1ms中断一次
	if(ERROR == RTC_config())
	{ 
		Set_LED(1);
		Delay(2500);
		Set_LED(0);
		while(1);
		//NVIC_SystemReset();
	};
  /* Add your application code here */
  /* Insert 500 ms delay */
  rt_thread_delay(500);
	ICM40607_Init();
	//IST8310_Init();
	SBUS_UART_Initial();
	LED_Init();
	STmotor485Initial();
	//USB_UARTx_Initial();
	TX2_UARTx_Inital();
//	if(!CAN_SetupConfig(1000000))
		CAN_SetupConfig(500000);
	CAN_FilterConfig(0,0,0,CAN_Id_Extended,CAN_RTR_Data);
	//RTC_config();
	SensorAppInit();
//	SPI_DMA_Init();
  rt_thread_delay(10);
	Function_semaphore_init();
//	Function_mailbox_init();
	Function_msg_queue_init();
//	SampleTimer_Init();
	Function_Task_create();
  rt_thread_delay(10);	
	SampleTimer_Init();

//	dataresult = changevalue(olddata ,10);
	while(1)
	{
//		CANSendMsg(SendData,0x0f1b0101,CAN_Id_Extended,CAN_RTR_Data,8);
//		Read_MAG3axis( &Mag_sensor);
//		ReverseMagPor(FALSE);
//		Delay(10);
//		Read_MAG3axis( &ReMag_sensor);
//		ReverseMagPor(TRUE);
//		rt_mb_send(CAN_Send_mailbox, (rt_ubase_t)(0x12345678));
		
		Set_LED(0);
		rt_thread_delay(950);
		Set_LED(1);
		rt_thread_delay(50);

				//获取日期和时间
//		RTC_GetDate(RTC_Format_BIN,&GetDateStruct);//获取当前日期
//		RTC_GetTime(RTC_Format_BIN, &GetTimeStrct);//获取当前时间
//		RTC_SubSecond = 1000-1000*RTC_GetSubSecond()/0xFF;//获取亚秒
//		rt_mb_send(CAN_Send_mailbox, (sendteset ++ ));
//get system time stamp
		systime = getsystimestamp();
	};

}

float getsystimestamp(){
		RTC_TimeTypeDef gettimestrct;//获取时间结构体
		u32 RTC_subsecond;//亚秒
	
		RTC_GetTime(RTC_Format_BIN, &gettimestrct);//获取当前时间
		RTC_subsecond = 1000-1000*RTC_GetSubSecond()/0xFF;//获取亚秒
		return (gettimestrct.RTC_Hours * 3600 + 
						gettimestrct.RTC_Minutes * 60 + 
						gettimestrct.RTC_Seconds + 
						((float)(RTC_subsecond)/1000));
}


/**
  * @brief  Inserts a delay time.
  * @param  nTime: specifies the delay time length, in milliseconds.
  * @retval None
  */
//void Delay(__IO uint32_t nTime)
//{ 
//  uwTimingDelay = nTime;

//  while(uwTimingDelay != 0);
//}

void Delay(u16 nTime)
{
	rt_thread_delay( nTime);
}

/**
  * @brief  Decrements the TimingDelay variable.
  * @param  None
  * @retval None
  */
void TimingDelay_Decrement(void)
{
  if (uwTimingDelay != 0x00)
  { 
    uwTimingDelay--;
  }
}

#ifdef  USE_FULL_ASSERT

/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t* file, uint32_t line)
{ 
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

  /* Infinite loop */
  while (1)
  {
  }
}
#endif
/** 
  * 函数功能: 超时等待函数
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明: 发送数据之后,等待从机响应,200ms之后则认为超时
  */
void WaitTimeOut(void )
{
  uint16_t TimeOut  = 0;// 通讯超时 单位:ms
  TimeOut = TIME_OVERRUN; // 定义超时时间为100ms,但实际测试时间为200ms
  while(Rx_MSG != MSG_COM)
  {
    Delay(1);
    if(TimeOut-- == 0)
    {
      if(Rx_MSG != MSG_COM)     // 200ms后还是没有接受数据，则认为超时
      {
        ResponTimeOut_cnt++;
      }
      return ;
    }
  }
	
}



void Function_Task_create (void)
{

//		SampleTimer_Init();
	

     tid1 = rt_thread_create("SensorTask",												//SensorTask
														SensorTask_entry, 									//thread function entry
														RT_NULL,														//thread param	
                            SENSOR_THREAD_STACK_SIZE,
                            SENSOR_THREAD_PRIO, 10);

    /* SensorTask thread startup */
    if (tid1 != RT_NULL)
        rt_thread_startup(tid1);
		
		tid2 = rt_thread_create("MotionTask",												//MotionTask
                            MotionTask_entry, 									//thread function entry
														RT_NULL,														//thread param	
                            MOTION_THREAD_STACK_SIZE,
                            MOTION_THREAD_PRIO, 10);

    /* MotionTask thread startup */
    if (tid2 != RT_NULL)
        rt_thread_startup(tid2);	
		
//				tid3 = rt_thread_create("MagnectTask",									//MagnectTask
//                            MagnectTask_entry, 									//thread function entry
//														RT_NULL,														//thread param	
//                            MAGNECT_THREAD_STACK_SIZE,
//                            MAGNECT_THREAD_PRIO, 10);

//    /* MagnectTask thread startup */
//    if (tid3 != RT_NULL)
//        rt_thread_startup(tid3);
		
		tid3 = rt_thread_create("STUartSendTask",									//STUartSendTask
                            STUart_SendTask_entry, 									//thread function entry
														RT_NULL,														//thread param	
                            STUARTSEND_THREAD_STACK_SIZE,
                            STUART_SEND_THREAD_PRIO, 10);

    /* STUartSendTask thread startup */
    if (tid3 != RT_NULL)
        rt_thread_startup(tid3);
		
//		tid4 = rt_thread_create("STUartRecTask",									//STUartRecTask
//                            STUart_RecTask_entry, 									//thread function entry
//														RT_NULL,														//thread param	
//                            STUARTREC_THREAD_STACK_SIZE,
//                            STUART_REC_THREAD_PRIO, 10);

//    /* STUartRecTask thread startup */
//    if (tid4 != RT_NULL)
//        rt_thread_startup(tid4);
		
//		tid5 = rt_thread_create("MonitorTask",									//MonitorTask
//                            MonitorTask_entry, 									//thread function entry
//														RT_NULL,														//thread param	
//                            MONITOR_THREAD_STACK_SIZE,
//                            MONITOR_THREAD_PRIO, 10);

//    /* MonitorTask thread startup */
//    if (tid5 != RT_NULL)
//        rt_thread_startup(tid5);	
		
				
//		tid6 = rt_thread_create("CAN_SendTask",									//CAN_SendTask
//                            CAN_SendTask_entry, 									//thread function entry
//														RT_NULL,														//thread param	
//                            CAN_SEND_THREAD_STACK_SIZE,
//                            CAN_SEND_THREAD_PRIO, 10);

//    /* CAN_SendTask thread startup */
//    if (tid6 != RT_NULL)
//        rt_thread_startup(tid6);
		
//			tid7 = rt_thread_create("CAN_RecTask",									//CAN_ReceiveTask
//                            CAN_RecTask_entry, 									//thread function entry
//														RT_NULL,														//thread param	
//                            CAN_REC_THREAD_STACK_SIZE,
//                            CAN_REC_THREAD_PRIO, 10);

//    /* CAN_RecTask thread startup */
//    if (tid7 != RT_NULL)
//        rt_thread_startup(tid7);
		
}

void Function_semaphore_init(void)
{
		rt_sem_sensor = rt_sem_create("sensor_sem", 0, RT_IPC_FLAG_PRIO);//create sensortask sem
		rt_sem_motion = rt_sem_create("motion_sem", 0, RT_IPC_FLAG_PRIO);//create motiontask sem
		rt_sem_magnect = rt_sem_create("magnect_sem", 0, RT_IPC_FLAG_PRIO);//create magnecttask sem
		rt_sem_monitor = rt_sem_create("monitor_sem", 0, RT_IPC_FLAG_PRIO);//create monitortask sem
		rt_sem_CANsendempty = rt_sem_create("CANsend_sem", 1, RT_IPC_FLAG_PRIO);//create CANSendempty sem
		rt_sem_IMUPrintPause = rt_sem_create("rt_sem_IMUPrintPause", 1, RT_IPC_FLAG_PRIO);//create rt_sem_IMUPrintPause sem
		rt_sem_STUartSendempty = rt_sem_create("STUart_send_sem", 1, RT_IPC_FLAG_PRIO);//create STUart_send_sem sem
	
}
/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
//void Function_mailbox_init(void)
//{
//	CAN_Send_mailbox = rt_mb_create("CANSendMailbox",					//	mailbox name
//																	15,											//	mailbox size
//																	RT_IPC_FLAG_FIFO);				//	mailbox flag
//	STUartRx_mailbox = rt_mb_create("STUartRxmailbox",					//	mailbox name
//																	2,											//	mailbox size
//																	RT_IPC_FLAG_FIFO);				//	mailbox flag
	
//}
	
/**
  * @name				Function_msg_queue_init
  * @brief			
  * @param  		none
  * @retval 		none
  */
void Function_msg_queue_init(void)
{
	rt_mq_CAN_Send =  rt_mq_create("CANSendMsgQueue",
											 sizeof (CanTxMsg),
											CAN_SEND_QUEUE_SIZE,
											 RT_IPC_FLAG_FIFO);
											
	rt_mq_CAN_Rec =  rt_mq_create("CANRecMsgQueue",
											 sizeof (CanRxMsg),
											CAN_REC_QUEUE_SIZE,
											 RT_IPC_FLAG_FIFO);
											 
	rt_mq_STmotor_send =  rt_mq_create("ST_UartSendMsgQueue",
											ST_UARTSEND_MSG_SIZE,//sizeof (ST_UartSendMsg),
											ST_UART_SEND_QUEUE_SIZE,
											 RT_IPC_FLAG_FIFO);					
											 
	rt_mq_STmotor_rec =  rt_mq_create("ST_UartRecMsgQueue",
											ST_UARTREC_MSG_SIZE,
											ST_UART_REC_QUEUE_SIZE,
											 RT_IPC_FLAG_FIFO);													 											 
}

// void IMU_Print_entry(void* parameter)
// {
//	 while(1)
//	 {
//		 if( rt_sem_take(rt_sem_IMUPrintPause,0) == RT_EOK )
//		 {
//			 rt_thread_suspend(tid7);//
//			 rt_schedule();
//		 }
//		 else
//		 {
//			 rt_kprintf("IMU Pitch %.2f\n",
//																							g_tSysStatus.sensor_info.fused_pitch_angle
////																						  g_tSysStatus.sensor_info.fused_roll_angle,
////																							g_tSysStatus.sensor_info.fused_yaw_angle,	
////																							g_tSysStatus.sensor_info.pitch_angle_rate,
////																							g_tSysStatus.sensor_info.roll_angle_rate
//																							);
////			 rt_kprintf("Print IMU value \n");
			 
//				rt_thread_delay(500);	
//		 }			 
//	 }
// }
/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void  reboot(void)
{
	__set_FAULTMASK(1);//shutdown all interrupts
	NVIC_SystemReset();//reboot system
}

void DBG_PrintStart(void)
{
	rt_thread_resume(tid7);
}

void DBG_PrintStop(void)
{
	rt_sem_release(rt_sem_IMUPrintPause);
}

//void MotionTask_Start(void)
//{
//    if (tid2 != RT_NULL)
//        rt_thread_startup(tid2);	

//}
/**	导出指令到MSH CMD	*/
MSH_CMD_EXPORT(reboot, soft restart MCU);
//MSH_CMD_EXPORT(MotionTask_Start, Put MotionTask into startup);

//MSH_CMD_EXPORT(DBG_PrintStart, start print IMU);
//MSH_CMD_EXPORT(DBG_PrintStop, stop print IMU);

int fputc(int ch, FILE *f)
{
	USART_SendData(USART1, (unsigned char) ch);// USART1 作为Printf打印口
	while (!(USART1->SR & USART_FLAG_TXE));
	return (ch);
}


//int fgetc (FILE *f) {

//while (!(TX2_UARTx->SR & USART_FLAG_RXNE));

//return ((int)(TX2_UARTx->DR & 0x1FF));

//}